/*
 * LineSensorArray.cpp
 *
 *  Created on: Jan 14, 2011
 *      Author: Robotics
 */

#include "LineSensorArray.h"

LineSensorArray::LineSensorArray(): m_leftSensor(4,5), m_middleSensor(4,6), m_rightSensor(4,7) {
	m_prefersRight = true;
	m_lastPosition = 0;

}

LineSensorArray::~LineSensorArray() {
	
}

void LineSensorArray::setPrefersRight(bool choice){
	m_prefersRight = choice;
}
/*Tells which sensor is over the line so you can tell the position of robot.  
 *You get back one number on the scale from -3 to 3.  Meaning the robot is all 
 *the  way on the right side for 3, and -3 would be all the way to the left. 
 */

int LineSensorArray::getPosition(){
	bool l = m_leftSensor.Get();
	bool m = m_middleSensor.Get();
	bool r = m_rightSensor.Get();
	
	int floorVisionMask = l<<2|m<<1|r;
	//............
	int ret = 0;
	switch ( floorVisionMask ) {
	  case 0 :
		//off the line
		  if(m_lastPosition < 0){
			  ret = -3;
		  }
		  if(m_lastPosition > 0){
			  ret = 3;
		  }
		  if(m_lastPosition == 0){
			ret = m_prefersRight? -3:3;	  
		  }
		  break;
	  case 1 :
	    //
		  ret= -2;
	    break;
	  case 2 :
		//middle
		  ret= 0;
	    break;
	  case 3 :
	  	//
	      ret= -1;
	  	 break;
	  case 4 :
	  	//
	  	  ret= 2;
	    break;
	  case 5 :
	 	//
	 	 ret= m_prefersRight? -2:2;
	   break;
	  case 6 :
	 	//
	 	  ret= 1;
	 	break;
	  case 7 :
	 	//
	 	  ret= m_prefersRight? -1:1;
	    break;
	  default :
	    // Process for all other cases.
		  break;
	}
	 m_lastPosition = ret;
	 return ret;
}

/*
 * function for getting the value of just one specific sensor.
 */

bool LineSensorArray::getLeftSensor(){
	return m_leftSensor.Get();
	
}

bool LineSensorArray::getMiddleSensor(){
	return m_middleSensor.Get();
	
}

bool LineSensorArray::getRightSensor(){
	return m_rightSensor.Get();
	
}

double LineSensorArray::PIDGet(){
	return getPosition();
}




